include("Operators.jl")
using FastGaussQuadrature

function MeshReaderGambit2D(filename)
    """
    Purpose : Read in basic grid information to build grid
    NOTE    : gambit(Fluent, Inc) *.neu format is assumed
    """
    # Open the .neu format meshfile
    file = open(filename)

    # Neglect the head of .neu file
    for i = 1:6
        line = readline(file)
    end

    # Get number of nodes, cells
    dim = parse.(Int, split(strip(readline(file)),r"\s+"))
    Nv = dim[1]; K = dim[2]

    # Skip two lines
    for i = 1:2
        line = readline(file)
    end

    # Read coordinates of the nodes
    Vx = [ ]; Vy = [ ]
    for i in 1:Nv
        line = parse.(Float64,split(strip(readline(file), ' '),r"\s+"))
        append!(Vx, line[2]); append!(Vy, line[3])
    end

    # Skip two lines
    for i = 1:2
        line = readline(file)
    end

    # Read elements to nodes connectivity
    EtoV = zeros(Int, K, 3)
    for i = 1:K
        line = parse.(Int,split(strip(readline(file), ' '),r"\s+"))
        EtoV[i,1] = line[4]; EtoV[i,2] = line[5]; EtoV[i,3] = line[6]
    end

    # Skip the material property section
    for i = 1:4
        line = readline(file)
    end
    test = intersect(line, ["ENDOFSECTION"])
    while isempty(test)
        line = split(strip(readline(file), ' '),r"\s+")
        test = intersect(line, ["ENDOFSECTION"])
    end

    line = readline(file); line = readline(file)

    # Define boundary type
    BCType = zeros(Int, K,3)
    In = 1; Out = 2; Wall = 3; Far = 4; Cyl = 5; Dirichlet = 6; Neuman = 7; Slip = 8

    while ~isempty(line)
        if ~isempty(intersect(split(strip(line, ' '),r"\s+"), ["In"])) bcflag = In end
        if ~isempty(intersect(split(strip(line, ' '),r"\s+"), ["Out"])) bcflag = Out end
        if ~isempty(intersect(split(strip(line, ' '),r"\s+"), ["Wall"])) bcflag = Wall end
        if ~isempty(intersect(split(strip(line, ' '),r"\s+"), ["Far"])) bcflag = Far end
        if ~isempty(intersect(split(strip(line, ' '),r"\s+"), ["Cyl"])) bcflag = Cyl end
        if ~isempty(intersect(split(strip(line, ' '),r"\s+"), ["Dirichlet"])) bcflag = Dirichlet end
        if ~isempty(intersect(split(strip(line, ' '),r"\s+"), ["Neuman"])) bcflag = Neuman end
        if ~isempty(intersect(split(strip(line, ' '),r"\s+"), ["Slip"])) bcflag = Slip end

        line = split(strip(readline(file), ' '),r"\s+")
        while isempty(intersect(line, ["ENDOFSECTION"]))
            boundary_id = parse.(Int, line)
            BCType[boundary_id[1], boundary_id[3]] = bcflag
            line = split(strip(readline(file), ' '),r"\s+")
        end
        line = readline(file); line = readline(file)
    end

    close(file)
    Nv, K, Vx, Vy, EtoV, BCType
end

function Warpfactor(N, rout)
    """
    Purpose: Compute scaled warp function at order N based on rout interpolation nodes
    """
    LGLr = gausslobatto(N+1)[1]; req = range(-1, stop = 1, length = N+1)

    # Compute Vandermonde matrix V based on req
    V = Vandermonde1D(N, req)

    # Evaluate Lagrange polynomial at rout
    Nr = length(rout); Pmat = zeros(N+1,Nr)
    for i = 1:N+1
        Pmat[i,:] = LegendreP(rout, i-1)'
    end
    Lmat = V'\Pmat

    # Compute warp factor
    warp = Lmat'*(LGLr - req)

    # Scale factor
    zerof = abs.(rout) .< 1.0 .- 1.0e-10
    sf = 1.0 .- (zerof.*rout).^2
    warp = warp./sf + warp.*(zerof .- 1.0)

    warp
end

function Nodes(N)
    """
    Purpose: Compute (x,y) nodes in equilateral triangle for polynomial of order N
    """
    alpopt = [0.0000, 0.0000, 1.4152, 0.1001, 0.2751, 0.9800, 1.0999, 1.2832, 1.3648,
              1.4773, 1.4959, 1.5743, 1.5770, 1.6223, 1.6258]

    # Set optimized parameter, alpha, depending on order N
    if N < 16
        alpha = alpopt[N]
    else
        alpha = 5.0/3.0
    end

    # Total number of Nodes
    Np = convert(Int, (N+1)*(N+2)/2)

    # Create equidistributed nodes on equilateral triangle
    L1 = zeros(Np,1); L2 = zeros(Np,1); L3 = zeros(Np,1); sk = 1
    for n = 1:N+1
        for m = 1:N+2-n
            L1[sk] = (n-1)/N; L3[sk] = (m-1)/N
            sk = sk + 1
        end
    end
    L2 = 1.0.-(L1 + L3); x = L3 - L2; y = (2*L1 - L2 - L3)/sqrt(3.0)

    # Compute blending function at each node for each edge
    blend1 = 4*L2.*L3; blend2 = 4*L1.*L3; blend3 = 4*L1.*L2

    # Amount of warp for each node, for each edge
    warpf1 = Warpfactor(N,L3-L2)
    warpf2 = Warpfactor(N,L1-L3)
    warpf3 = Warpfactor(N,L2-L1)

    # Combine blend & warp
    warp1 = blend1.*warpf1.*(1 .+ (alpha*L1).^2)
    warp2 = blend2.*warpf2.*(1 .+ (alpha*L2).^2)
    warp3 = blend3.*warpf3.*(1 .+ (alpha*L3).^2)


    # Accumulate deformations associated with each edge
    x = x + 1*warp1 + cos(2*pi/3)*warp2 + cos(4*pi/3)*warp3
    y = y + 0*warp1 + sin(2*pi/3)*warp2 + sin(4*pi/3)*warp3

    x,y
end

function GeometricFactors2D(x,y,Dr,Ds)
    """
    Purpose  : Compute the metric elements for the local mappings of the elements
    """
    # Calculate geomatric factors
    xr = Dr*x; xs = Ds*x; yr = Dr*y; ys = Ds*y
    J = -xs.*yr + xr.*ys
    rx = ys./J; sx =-yr./J; ry =-xs./J; sy = xr./J
    rx,sx,ry,sy,J
end

function Normals2D(x,y,Dr,Ds,Fmask,Nfp,K)
    """
    Purpose : Compute outward pointing normals at elements faces and surface Jacobians
    """
    xr = Dr*x; xs = Ds*x; yr = Dr*y; ys = Ds*y
    J = -xs.*yr + xr.*ys

    # Interpolate geometric factors to face nodes
    fxr = xr[Fmask[:], :]; fxs = xs[Fmask[:], :]
    fyr = yr[Fmask[:], :]; fys = ys[Fmask[:], :]

    # Build normals
    nx = zeros(3*Nfp, K); ny = zeros(3*Nfp, K)
    fid1 = 1:Nfp; fid2 = Nfp .+ 1:2*Nfp; fid3 = 2*Nfp .+ 1:3*Nfp

    # Face 1
    nx[fid1, :] =  fyr[fid1, :]; ny[fid1, :] = -fxr[fid1, :]

    # Face 2
    nx[fid2, :] =  fys[fid2, :] - fyr[fid2, :]
    ny[fid2, :] = -fxs[fid2, :] + fxr[fid2, :]

    # Face 3
    nx[fid3, :] = -fys[fid3, :]; ny[fid3, :] =  fxs[fid3, :]

    # Normalize
    sJ = sqrt.(nx.*nx + ny.*ny); nx = nx./sJ; ny = ny./sJ

    nx, ny, sJ
end

function tiConnect2D(EToV,Nfaces,K,Nv)
    """
    Purpose: triangle face connect algorithm due to Toby Isaac
    """
    # Create list of all faces 1, then 2, & 3
    fnodes = [EToV[:,[1,2]]; EToV[:,[2,3]];EToV[:,[3,1]]]
    fnodes = sort(fnodes, dims = 2) .- 1

    # Set up default element to element and Element to faces connectivity
    EToE= collect(1:K)*ones(Int,1,Nfaces)
    EToF= ones(Int,K,1)*(1:Nfaces)'

    # Uniquely number each set of three faces by their node numbers
    id = fnodes[:,1]*Nv + fnodes[:,2] .+ 1
    spNodeToNode = [id collect(1:Nfaces*K) EToE[:] EToF[:]]
    sorted = sortslices(spNodeToNode, dims=1)

    # Find matches in the sorted face list
    indices = findall(sorted[1:end-1,1].== sorted[2:end,1])

    # make links reflexive
    matchL = [sorted[indices,:]  ; sorted[indices .+ 1,:]]
    matchR = [sorted[indices .+ 1,:]; sorted[indices,:]]

    # insert matches
    EToE[matchL[:,2]] = matchR[:,3]
    EToF[matchL[:,2]] = matchR[:,4]

    EToE, EToF
end

function BuildMaps2D(N,K,Fmask,EToE,EToF,EToV,Vx,Vy,x,y)
    """
    Purpose: Connectivity and boundary tables in the K # of Np elements
    """
    # Definition of constants
    Nfaces=3; Nfp = N+1; Np = convert(Int, (N+1)*(N+2)/2); NODETOL = 1e-12

    # Number volume nodes consecutively
    nodeids = reshape(1:K*Np, Np, K)
    vmapM   = zeros(Int, Nfp, Nfaces, K)
    vmapP   = zeros(Int, Nfp, Nfaces, K)
    mapM    = collect(Int, 1:K*Nfp*Nfaces)
    mapP    = reshape(collect(Int, 1:K*Nfp*Nfaces),Nfp, Nfaces, K)

    # Find index of face nodes with respect to volume node ordering
    for k1=1:K
        for f1=1:Nfaces
            vmapM[:,f1,k1] = nodeids[Fmask[:,f1], k1]
        end
    end

    one = ones(1, Nfp);
    for k1=1:K
      for f1=1:Nfaces
        # Find neighbor
        k2 = EToE[k1,f1]; f2 = EToF[k1,f1]

        # Reference length of edge
        v1 = EToV[k1,f1]; v2 = EToV[k1, 1 + mod(f1,Nfaces)]
        refd = sqrt.((Vx[v1]-Vx[v2]).^2 + (Vy[v1]-Vy[v2]).^2)

        # Find find volume node numbers of left and right nodes
        vidM = vmapM[:,f1,k1]; vidP = vmapM[:,f2,k2]
        x1 = x[vidM]; y1 = y[vidM]; x2 = x[vidP]; y2 = y[vidP]
        x1 = x1*one;  y1 = y1*one;  x2 = x2*one;  y2 = y2*one

        # Compute distance matrix
        D = (x1 -x2').^2 + (y1-y2').^2

        index = findall(sqrt.(abs.(D)) .< NODETOL*refd)
        idM = [index[i][1] for i = 1:length(index)]
        idP = [index[i][2] for i = 1:length(index)]

        vmapP[idM,f1,k1] = vidP[idP]
        mapP[idM,f1,k1] = idP .+ ((f2-1)*Nfp+(k2-1)*Nfaces*Nfp)
      end
    end

    # Reshape vmapM and vmapP to be vectors and create boundary node list
    vmapP = vmapP[:]; vmapM = vmapM[:]; mapP = mapP[:]
    mapB = findall(vmapP .== vmapM); vmapB = vmapM[mapB]

    mapM, mapP, vmapM, vmapP, vmapB, mapB
end

function BuildBCMaps2D(Nfp,BCType,vmapM)
    """
    Purpose: Build specialized nodal maps for various types of
    boundary conditions, specified in BCType.
    """
    #Create label of face nodes with boundary types from BCType
    bct    = BCType'
    bnodes = ones(Nfp, 1)*bct[:]'
    bnodes = bnodes[:]

    # Find location of boundary nodes in face and volume node lists
    In = 1; Out = 2; Wall = 3; Far = 4; Cyl = 5; Dirichlet = 6; Neuman = 7; Slip = 8
    mapI = findall(bnodes .== In);           vmapI = vmapM[mapI]
    mapO = findall(bnodes .== Out);          vmapO = vmapM[mapO]
    mapW = findall(bnodes .== Wall);         vmapW = vmapM[mapW]
    mapF = findall(bnodes .== Far);          vmapF = vmapM[mapF]
    mapC = findall(bnodes .== Cyl);          vmapC = vmapM[mapC]
    mapD = findall(bnodes .== Dirichlet);    vmapD = vmapM[mapD]
    mapN = findall(bnodes .== Neuman);       vmapN = vmapM[mapN]
    mapS = findall(bnodes .== Slip);         vmapS = vmapM[mapS]

    mapI, mapO, mapW, mapF, mapC, mapD, mapN, mapS
end

function InitCondition(Np,K,x,y,time,simtype)
    """
    Purpose: compute flow configuration given by
    Y.C. Zhou, G.W. Wei / Journal of Computational Physics 189 (2003) 159
    """
    if simtype == "InsentroicVortex"
        # based flow parameters
        xo = 5.0; yo = 0; beta = 5.0; gamma = 1.4
        rho = 1.0; u = 1.0; v = 0.0; p = 1.0

        xt = x .- u*time; yt = y .- v*time
        r = sqrt.((xt .- xo).^2 + (yt .- yo).^2)

        # Perturbed density
        u  = u .- beta*exp.(1.0 .- r.^2).*(yt .- yo)/(2*pi)
        v  = v .+ beta*exp.(1.0 .- r.^2).*(xt .- xo)/(2*pi)
        rho1 = (1 .- ((gamma-1)*beta^2*exp.(2*(1.0 .- r.^2))/(16*gamma*pi*pi))).^(1/(gamma-1))
        p1   = rho1.^gamma

        # Set initial solutions
        Q = zeros(Float64, Np,K,4)
        Q[:,:,1] = rho1; Q[:,:,2] = rho1.*u; Q[:,:,3] = rho1.*v
        Q[:,:,4] = p1/(gamma-1) + 0.5*rho1.*(u.^2 + v.^2)

        Q
    else
        print("Not implement yet!\n")
        exit()
    end
end

function  ExactSolutionBC(Q,Np,K,Fx,Fy,nx,ny,mapI,mapO,mapW,mapC,QP,time)
    """
    Purpose: Impose boundary conditions on 2D Euler equations on weak form
    """
    Qbc = InitCondition(Np,K,Fx,Fy,time, "InsentroicVortex")

    mapB = [mapI;mapO;mapW]

    for n=1:4
        Qn = Q[:,:,n];  Qbcn = Qbc[:,:,n]; Qn[mapB] = Qbcn[mapB]
        Q[:,:,n] = Qn
    end

    Q
end
